#include "Communication/InterruptAndRecoveryCom.h"

namespace behavior_controller
{
WaitAndInsertPose insert_pose_client;
void WaitAndInsertPose::SetNode(const rclcpp::Node::SharedPtr &ros_node)
{
    if(ros_node_ == nullptr)
    {
        ros_node_=ros_node;
        client_nav = ros_node_->create_client<nav2_msgs::srv::WaitAndInsertPose>("/local_planner/insert_pose");
    }
}

bool WaitAndInsertPose::async_send_request(std::string name, double length,bool start,geometry_msgs::msg::PoseStamped  pose)
{
    std::cout<<"WaitAndInsertPose, from " <<name<< std::endl;
    std::vector<std::vector<bool> > grid;
    auto req_srv =  std::make_shared<nav2_msgs::srv::WaitAndInsertPose::Request>();
    req_srv->length=length;
    req_srv->start=start;
    req_srv->pose=pose;
    std::cout<<"Requesting WaitAndInsertPose!" << std::endl;
    if(!client_nav->wait_for_service(std::chrono::seconds(3)))
    {
        std::cout<<"waiting 5s more..." << std::endl;
        if(!client_nav->wait_for_service(std::chrono::seconds(5)))
        {
            return false;
        }
    }
    auto result_future= client_nav->async_send_request(req_srv);
    return result_future.get()->result;
}
}

